/**
 * @file step_motor.c
 * @author Neozng's little fan
 * @brief 步进电机控制
 * @version 0.5
 * @date 2023-12-21
 *
 * @copyright Copyright (c) 2023
 *
 */

#include "step_motor.h"
#include "stdlib.h"
#include "memory.h"
#include "bsp_pwm.h"
#include "bsp_dwt.h"
#include "tim.h"
static StepInstance *step_motor_instance[STEP_MOTOR_CNT] = {NULL};
static uint8_t step_idx = 0; // register servo_idx,是该文件的全局步进电机索引,在注册时使用
//static PWMInstance *stepmotor_pwm_instance;  //步进电机拥有的pwm实例

/**
 * @brief pwm比较输出模式下的中断回调函数，速度和角度
 *
 * @note 内部函数，需要调用实例，封装一层是为了使module与bsp解耦
 */
// uint32_t DWT_CNT1;
// float dt1;
// uint32_t DWT_CNT2;
// float dt2;
//dwt测时间

void OC_Callback_SA_in(StepInstance *Step_Motor)
{
    int16_t current_value = 0;

    Step_Motor->count++;
    current_value = __HAL_TIM_GetCompare(Step_Motor->htim,Step_Motor->Channel);
    __HAL_TIM_SetCompare(Step_Motor->htim,Step_Motor->Channel,(current_value + Step_Motor->sareturn.pulse_ccr)%0XFFFF);
    
    // if(Step_Motor->htim == &htim4 ) dt1 = DWT_GetDeltaT((void *)&DWT_CNT1);
    // if(Step_Motor->htim == &htim8 ) dt2 = DWT_GetDeltaT((void *)&DWT_CNT2);
    
    Step_Motor->sareturn.pulse_count = Step_Motor->sareturn.pulse_count - 0.5;
    if(Step_Motor->dir_flag == 1)
    {
        Step_Motor->abs_pulsecount = Step_Motor->abs_pulsecount + 0.5;
        Step_Motor->abs_angle = (Step_Motor->abs_pulsecount) * (Step_Motor->Init_perangle);
    }
    else if(Step_Motor->dir_flag == 0)
    {
        Step_Motor->abs_pulsecount = Step_Motor->abs_pulsecount - 0.5;
        Step_Motor->abs_angle = (Step_Motor->abs_pulsecount) * (Step_Motor->Init_perangle);
    }
    if(Step_Motor->sareturn.pulse_count <= 0)
    {
      HAL_TIM_OC_Stop_IT(Step_Motor->htim,Step_Motor->Channel);
      Step_Motor->count = 0;
      Step_Motor->angle = 0;
    }   
}

/**
 * @brief pwm比较输出模式下的中断回调函数（速度和角度）的封装
 */
void OC_Callback_SA(PWMInstance* pwm)
{
    for (uint8_t i = 0; i < step_idx; i++)
    {
        // 来自同一个定时器的中断且通道相同
        if (step_motor_instance[i]->pwm_instance->htim== pwm->htim && step_motor_instance[i]->pwm_instance->id == pwm->id )
        {
            OC_Callback_SA_in(step_motor_instance[i]);
            return; // 一次只能有一个通道的中断,所以直接返回
        }
        
    }

}

/**
 * @brief pwm比较输出模式下的中断回调函数，仅速度
 *
 * @note 内部函数，需要调用实例，封装一层是为了使module与bsp解耦
 */
void OC_Callback_S_in(StepInstance *Step_Motor)
{
    int16_t current_value = 0;
    
    current_value = __HAL_TIM_GetCompare(Step_Motor->htim,Step_Motor->Channel);
    __HAL_TIM_SetCompare(Step_Motor->htim,Step_Motor->Channel,(current_value + Step_Motor->sareturn.pulse_ccr)%0XFFFF);
}

/**
 * @brief pwm比较输出模式下的中断回调函数（仅速度）的封装
 */
void OC_Callback_S()
{
    for (uint8_t i = 0; i < step_idx; i++)
    {
        OC_Callback_S_in(step_motor_instance[i]);
        return; // 一次只能有一个通道的中断,所以直接返回
    }
}

/**
 * @brief 用于处理梯形加减速的终端回调函数（内部）
 * 
 * @note 内部函数，需要调用实例，封装一层是为了使module与bsp解耦
 */

void OC_Callback_Trape_in(StepInstance *Step_Motor)
{
    int16_t current_value = 0;
    TRAPE_ACCEL_STAT_t state;

    current_value = __HAL_TIM_GetCompare(Step_Motor->htim,Step_Motor->Channel);

    state = TrapeCal_CalNextCounterVal(&(Step_Motor->trape_medium));

    if(state == STOP)
    {
        HAL_TIM_OC_Stop_IT(Step_Motor->htim,Step_Motor->Channel);
    }
    else
    {
        __HAL_TIM_SetCompare(Step_Motor->htim,Step_Motor->Channel,(current_value + (Step_Motor->trape_medium.presentCounterVal) / 2 )%0XFFFF);
    }
}

/**
 * @brief 用于处理梯形加减速的终端回调函数封装
*/
void OC_Callback_Trape()
{
    for (uint8_t i = 0; i < step_idx; i++)
    {
        OC_Callback_Trape_in(step_motor_instance[i]);
        return; // 一次只能有一个通道的中断,所以直接返回
    }

}




/**
 * @brief 通过此函数注册一个步进电机
 *
 * @note 仿照遥控器处理回调函数的方式封装了一下，但是思路还是有点混乱
 */
StepInstance *StepInit(Step_Init_Config_s *Step_Init_Config)
{
    StepInstance *step = (StepInstance *)malloc(sizeof(StepInstance));
    memset(step, 0, sizeof(StepInstance)); //不知道包含指针可不可以用memset，暂且存疑
    //吸取教训，用指针前一定要先分配内存

    step->htim = Step_Init_Config->htim;
    step->Channel = Step_Init_Config->Channel;
    step->DIR = Step_Init_Config->DIR;
    step->ENA = Step_Init_Config->ENA;
    step->Init_frequency = Step_Init_Config->Init_frequency;
    step->Init_perangle = Step_Init_Config->Init_perangle;
    step->angle = Step_Init_Config->angle;
    step->speed = Step_Init_Config->speed;
    step->mode = Step_Init_Config->mode;
    step->count = Step_Init_Config->count;
    step->dir_flag = Step_Init_Config->dir_flag;
    

    step->trape_param.accelSpeed = Step_Init_Config->trape_param.accelSpeed;
    step->trape_param.ClkFrq = Step_Init_Config->Init_frequency;
    step->trape_param.constSpeed = Step_Init_Config->trape_param.constSpeed;
    step->trape_param.decelSpeed = Step_Init_Config->trape_param.decelSpeed;
    step->trape_param.stepsize = Step_Init_Config->Init_perangle;
    step->trape_param.total_step = Step_Init_Config->trape_param.total_step;
    step->abs_angle = 0;
    step->abs_pulsecount = 0;
    //似乎有冗余配置，但是因为计算需要先不改了
    Step_Init_Config->pwm_init_config.htim = step->htim;
    Step_Init_Config->pwm_init_config.channel = step->Channel;
    Step_Init_Config->pwm_init_config.id = step;
    switch (step->mode) 
    {
    case 1:
        Step_Init_Config->pwm_init_config.callback_oc = OC_Callback_S;
        break;
    case 2:
        Step_Init_Config->pwm_init_config.callback_oc = OC_Callback_SA; 
        break;
    case 3:
        Step_Init_Config->pwm_init_config.callback_oc = OC_Callback_Trape;
        //Step_Init_Config->trape_medium.presentCounterVal = TrapeCal_Start(&(Step_Init_Config->trape_medium));
        break;
    default:
        break;
    }
    step->pwm_instance = PWMRegisterOC(&Step_Init_Config->pwm_init_config);

    // //使能定时器以及ENA和DIR对应的引脚
    // //不太确定是否要写HAL_TIM_Base_Start();不确定重复开启定时器有什么问题；留待查验


    // // HAL_TIM_Base_Start(Step_Init_Config->htim);
    // // HAL_TIM_OC_Init(Step_Init_Config->htim); 
    //定时器的开启挪到bsp_Init初始化里了
    HAL_TIM_OC_Start_IT(Step_Init_Config->htim,Step_Init_Config->Channel);
    HAL_GPIO_WritePin(Step_Init_Config->ENA.GPIOx,Step_Init_Config->ENA.GPIO_Pin,0);
    HAL_GPIO_WritePin(Step_Init_Config->DIR.GPIOx,Step_Init_Config->DIR.GPIO_Pin,1);	
  
    step_motor_instance[step_idx++] = step;
    return step;
}

void Set_abs_SA(StepInstance *Step_Motor,float set_angle,float speed)
{
    Step_Motor->speed = speed;
    Step_Motor->set_angle = set_angle;
    Step_Motor->angle = Step_Motor->set_angle - Step_Motor->abs_angle;
    Rotate_Relative_SA(Step_Motor);
}

void Set_SA(StepInstance *Step_Motor,float speed,uint16_t mode)
{
    Step_Motor->speed = speed;
    Step_Motor->mode = mode;
    Rotate_Relative_SA(Step_Motor);
}


/**
 * @brief 转动相对角度
 *
 * @note 速度为负的时候还会有点问题，还没把逻辑改顺-->放弃了，先不带速度玩了
 */

void Rotate_Relative_SA(StepInstance *Step_Motor)
{
    Step_Motor->sareturn.pulse_count =   (Step_Motor->angle) / (Step_Motor->Init_perangle);  //脉冲个数，两次翻转一个脉冲，在中断里进行了处理
    if(Step_Motor->angle < 0)
    {
        HAL_GPIO_WritePin(Step_Motor->DIR.GPIOx,Step_Motor->DIR.GPIO_Pin,0);
        Step_Motor->sareturn.pulse_count = -(Step_Motor->sareturn.pulse_count);
        Step_Motor->dir_flag = 0;
    }
    else
    {
        HAL_GPIO_WritePin(Step_Motor->DIR.GPIOx,Step_Motor->DIR.GPIO_Pin,1);
        Step_Motor->dir_flag = 1;
    }
    
    Step_Motor->sareturn.pulse_ccr =  (int32_t)(Step_Motor->Init_frequency * Step_Motor->Init_perangle ) / (2.0 * Step_Motor->speed);
    //这是每次ccr的增量，但事实上增加两次才是转一个步距角。所以这里对算出的数值进行除以2的处理，这样算两次的时间正好对上
    if(Step_Motor->sareturn.pulse_count <= 0)
    {
        HAL_TIM_OC_Stop_IT(Step_Motor->htim,Step_Motor->Channel);
    }
    else  
    {
        HAL_TIM_OC_Start_IT(Step_Motor->htim,Step_Motor->Channel);
    }
}

void StepMotor_ModeSet(StepInstance *Step_Motor,uint16_t mode)
{
    Step_Motor->mode = mode;
}

void StepMotor_TrapeSet(StepInstance *Step_Motor,float angle,float constSpeed,float accelSpeed,float decelSpeed)
{
    if(angle < 0 || constSpeed < 0)
    {
        HAL_GPIO_WritePin(Step_Motor->DIR.GPIOx,Step_Motor->DIR.GPIO_Pin,0);
        if(angle < 0)
        {
            angle = -angle;
        }
        if(constSpeed < 0)
        {
            constSpeed = -constSpeed;
        }
    }

    Step_Motor->trape_param.total_step = (uint32_t)(angle / Step_Motor->trape_param.stepsize);
    Step_Motor->trape_param.accelSpeed = accelSpeed;
    Step_Motor->trape_param.decelSpeed = decelSpeed;
    Step_Motor->trape_param.constSpeed = constSpeed;

}

void StepMotor_TrapeSetStep(StepInstance *Step_Motor, uint32_t total_step)
{
    Step_Motor->trape_param.total_step = total_step;
}
